import pybullet as p
import time

# 初始化
physicsClient = p.connect(p.GUI)
p.setGravity(0, 0, -9.8)

# 创建地面
ground_shape = p.createCollisionShape(p.GEOM_PLANE)
ground = p.createMultiBody(0, ground_shape)

# 创建摆锤
sphere_radius = 0.2
sphere_collision = p.createCollisionShape(p.GEOM_SPHERE, radius=sphere_radius)
sphere_visual = p.createVisualShape(p.GEOM_SPHERE, radius=sphere_radius, rgbaColor=[1, 0, 0, 1])
sphere_mass = 1.0
sphere = p.createMultiBody(sphere_mass, sphere_collision, sphere_visual, [0, 0, 2])

# 使用点对点约束创建钟摆
# 将摆锤连接到世界上的一个固定点
constraint_id = p.createConstraint(
    parentBodyUniqueId=sphere,  # 连接到世界
    parentLinkIndex=-1,
    childBodyUniqueId=-1,
    childLinkIndex=-1, 
    jointType=p.JOINT_POINT2POINT,
    jointAxis=[0, 0, 0],
    parentFramePosition=[0, 0, 3],  # 世界坐标系中的固定点
    childFramePosition=[0, 0, 0]    # 球体坐标系中的连接点（中心）
)

# 添加一些阻尼使运动更稳定
p.changeConstraint(constraint_id, erp=0.8)

# 仿真循环
try:
    for i in range(1000):
        p.stepSimulation()
        time.sleep(1./240.)
except KeyboardInterrupt:
    pass

p.disconnect()